import sys
import rospy
from moveit_commander import MoveGroupCommander, roscpp_initialize
import moveit_commander
from geometry_msgs.msg import PoseStamped
from moveit_commander import PlanningSceneInterface
from rospy.rostime import get_time
# remap again
joint_state_topic = ['joint_states:=/j2n6s300/joint_states']
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('foo', anonymous=False)
roscpp_initialize(sys.argv)

right_arm = MoveGroupCommander('arm')
right_arm.set_max_acceleration_scaling_factor(1)
right_arm.set_max_velocity_scaling_factor(1)
# right_arm.set_joint_value_target([0,2.9,1.3,-2.07,1.4,0])
right_arm.set_pose_reference_frame("root")
scene = PlanningSceneInterface()


arm = right_arm
end_effector_link = arm.get_end_effector_link()
print(end_effector_link)
arm.set_start_state_to_current_state()
# 设置机械臂终端运动的目标位姿
print("OK")



def getpose(a,b):
    target_pose = PoseStamped()
    target_pose.header.frame_id = "root"
    target_pose.header.stamp = rospy.Time.now()
    target_pose.pose.position.x = a
    target_pose.pose.position.y = b
    target_pose.pose.position.z = 0.4
    target_pose.pose.orientation.x = 0
    target_pose.pose.orientation.y = 0
    target_pose.pose.orientation.z = 0
    target_pose.pose.orientation.w = 1
    return target_pose
for _ in range(10):
    # arm.shift_pose_target(1, -0.05, "j2n6s300_link_6")
    
    target_pose =getpose(0.1*_-0.5,0.1*_-0.5)
    arm.set_pose_target(target_pose, "j2n6s300_link_6")
    # # 规划运动路径
    # traj = arm.plan()
    # 按照规划的运动路径控制机械臂运动
    arm.go()


